#include <tf2/LinearMath/Quaternion.h>
#include <eigen3/Eigen/Core>
// #include "Eigen/Dense"
#include "Eigen/Geometry"
#include <iostream>

void GetQuatFromRpy(double roll, double pitch, double yaw) {
    tf2::Quaternion quat;
    quat.setRPY(roll, pitch, yaw); // 绕固定角旋转： xyz
    quat.normalized();
    std::cout<<"[tf2 from rpy]:"<<std::endl;
    std::cout << "w: " << quat.w() << std::endl;
    std::cout << "x: " << quat.x() << std::endl;
    std::cout << "y: " << quat.y() << std::endl;
    std::cout << "z: " << quat.z() << std::endl;
}

void GetQuatFromEuler(double roll, double pitch, double yaw) {
    // 这个接口没搞懂
    tf2::Quaternion quat2;
    quat2.setEuler(yaw, pitch, roll); // 绕自身坐标系旋转：zyx
    quat2.normalized();
    std::cout<<"[tf2 from euler]:"<<std::endl;
    std::cout << "w: " << quat2.w() << std::endl;
    std::cout << "x: " << quat2.x() << std::endl;
    std::cout << "y: " << quat2.y() << std::endl;
    std::cout << "z: " << quat2.z() << std::endl;
}

/**
 * @brief:弧度制欧拉角转四元数
 * @param {double} rx 绕x轴的旋转
 * @param {double} ry 绕y轴的旋转
 * @param {double} rz 绕z轴的旋转
 * @return {Quaterniond} 返回对应的四元数
 * 
 * @note： 这种是绕自身坐标系旋转的，也就是先绕x轴----再绕Y轴---最后绕Z轴;这相当于绕固定角的：YPR
 * 
 */
Eigen::Quaterniond Euler2QuatXYZ(double rx, double ry, double rz)
{
  return (Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX()) *  // 绕X轴旋转rx角度
         Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *  // 绕Y轴旋转ry角度
         Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ())).normalized();   // 绕Z轴旋转rz角度
}


/**
 * @brief:弧度制欧拉角转四元数
 * @param {double} rx 绕x轴的旋转
 * @param {double} ry 绕y轴的旋转
 * @param {double} rz 绕z轴的旋转
 * @return {Quaterniond} 返回对应的四元数
 * 
 * @note： 这种是绕自身坐标系旋转的，也就是先绕Z轴----再绕Y轴---最后绕X轴;这相当于绕固定角的：RPY
 * 
 */
Eigen::Quaterniond Euler2QuatZYX(double rx, double ry, double rz)
{
  // 反转顺序以匹配setRPY: Z → Y → X
  return (Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ()) *  // 先绕Z轴
         Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *  // 然后绕Y轴
         Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX())).normalized();   // 最后绕X轴
}

double Deg2Rad(double angle) { return angle / 180 * M_PI; }
double Rad2Deg(double angle) { return angle * 180 / M_PI; }

/**
 * @brief: 角度制欧拉角转四元数
 * @param {double} rx 绕x轴的旋转
 * @param {double} ry 绕y轴的旋转
 * @param {double} rz 绕z轴的旋转
 * @return {Quaterniond} 返回对应的四元数
 */
Eigen::Quaterniond EulerAngle2QuatXYZ(double rx, double ry, double rz)
{
  rx = Deg2Rad(rx);
  ry = Deg2Rad(ry);
  rz = Deg2Rad(rz);
  return (Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX()) *
         Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *
         Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ())).normalized();
}

Eigen::Quaterniond EulerAngle2QuatZYX(double rx, double ry, double rz)
{
  rx = Deg2Rad(rx);
  ry = Deg2Rad(ry);
  rz = Deg2Rad(rz);
  return (Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ()) *
         Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *
         Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX())).normalized();
}